/******************************************************************************

 @file sensor.c

 @brief TIMAC 2.0 Sensor Example Application

 Group: WCS LPC
 Target Device: cc13xx_cc26xx

 ******************************************************************************
 
 Copyright (c) 2016-2025, Texas Instruments Incorporated
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions
 are met:

 *  Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.

 *  Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

 *  Neither the name of Texas Instruments Incorporated nor the names of
    its contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 ******************************************************************************
 
 
 *****************************************************************************/

/******************************************************************************
 Includes
 *****************************************************************************/
 /* RTOS header files */
#include "ti/display/Display.h"
#ifndef FREERTOS_SUPPORT
#include <ti/sysbios/BIOS.h>
#else
#include <FreeRTOS.h>
#include <task.h>
#endif

#include <pthread.h>
#include <mqueue.h>

#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "mac_util.h"
#include "api_mac.h"
#include "jdllc.h"
#include "ssf.h"
#include "smsgs.h"
#include "sensor.h"
#include <advanced_config.h>
#include "packet_transmission.h"
#include "sensor_data_generator.h"
#include "data_sender.h"
#include "frame_control_queue.h"
#include "ti_154stack_config.h"

#ifdef FEATURE_NATIVE_OAD
#include "oad_client.h"
#endif /* FEATURE_NATIVE_OAD */

#ifdef OSAL_PORT2TIRTOS
#include <ti/drivers/dpl/ClockP.h>
#else
#include "icall.h"
#endif

#ifndef CUI_DISABLE
#include <stdint.h>
#include "cui.h"
#endif /* CUI_DISABLE */

#ifdef DEVICE_TYPE_MSG
#include <ti/devices/DeviceFamily.h>
#include "device_type.h"
#endif /* DEVICE_TYPE_MSG */

#ifdef FEATURE_SECURE_COMMISSIONING
#include "sm_ti154.h"
#include "hal_types.h"
#endif /* FEATURE_SECURE_COMMISSIONING */

#ifdef MAC_DUTY_CYCLE_CHECKING
#include "mac_duty_cycle/mac_duty_cycle.h"
#endif

#ifdef MAC_OVERRIDE_TX_DELAY
#include "mac_api.h"
#endif

#include <ti/log/Log.h>
#include "util_timer.h"
#include <ti/drivers/Power.h>
#include <ti/drivers/power/PowerCC26XX.h>
#include <ti/devices/DeviceFamily.h>
#include DeviceFamily_constructPath(driverlib/sys_ctrl.h)

/******************************************************************************
 Constants and definitions
 *****************************************************************************/
#define TAG "Sensor"
#ifndef DEBUG_PRINT_ENABLE
#define DEBUG_PRINT_ENABLE 1
#endif
#if DEBUG_PRINT_ENABLE
// Macro for debug print with file, function, and line number
// #define DEBUG_PRINT(fmt, ...) \
//     Display_printf(display, 0, 0, "[%s:%d:%s] " fmt, TAG, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define DEBUG_PRINT(fmt, ...) \
    Display_printf(display, 0, 0, "[%s:%d] " fmt, TAG, __LINE__, ##__VA_ARGS__)
#else
#define DEBUG_PRINT(fmt, ...)
#endif

#if !defined(CONFIG_AUTO_START)
#if defined(AUTO_START)
#define CONFIG_AUTO_START 1
#else
#define CONFIG_AUTO_START 0
#endif
#endif

/* default MSDU Handle rollover */
#define MSDU_HANDLE_MAX 0x1F

/* App marker in MSDU handle */
#define APP_MARKER_MSDU_HANDLE 0x80

/* App Message Tracking Mask */
#define APP_MASK_MSDU_HANDLE 0x60

/* App Sensor Data marker for the MSDU handle */
#define APP_SENSOR_MSDU_HANDLE 0x40

/* App tracking response marker for the MSDU handle */
#define APP_TRACKRSP_MSDU_HANDLE 0x20

/* App config response marker for the MSDU handle */
#define APP_CONFIGRSP_MSDU_HANDLE 0x60

/* Reporting Interval Min and Max (in milliseconds) */
#define MIN_REPORTING_INTERVAL 1000
#define MAX_REPORTING_INTERVAL 3600000

/* Polling Interval Min and Max (in milliseconds) */
#define MIN_POLLING_INTERVAL 1
#define MAX_POLLING_INTERVAL 100000

/* Blink Time for Identify LED Request (in seconds) */
#define IDENTIFY_LED_TIME 1

/* Inter packet interval in certification test mode */
#if CERTIFICATION_TEST_MODE
#define CERT_MODE_INTER_PKT_INTERVAL 1000
#endif

/******************************************************************************
 Global variables
 *****************************************************************************/
/* MAC's IEEE address. This is only for Sensor */
extern ApiMac_sAddrExt_t ApiMac_extAddr;

/* Task pending events */
uint16_t Sensor_events = 0;

/* accumulated total E2E delay */
uint32_t totalE2EDelaySum = 0;

/* saved end to end delay */
uint32_t endToEndDelay = 0;

/*! Sensor statistics */
Smsgs_msgStatsField_t Sensor_msgStats =
    { 0 };
extern bool initBroadcastMsg;
extern bool parentFound;

#ifdef POWER_MEAS
/*! Power Meas Stats fields */
Smsgs_powerMeastatsField_t Sensor_pwrMeasStats =
    { 0 };
#endif

#ifndef CUI_DISABLE
extern CUI_clientHandle_t ssfCuiHndl;
#endif
extern uint32_t sensorStatusLine;
/******************************************************************************
 Local variables
 *****************************************************************************/

static void *sem;

/*! Rejoined flag */
static bool rejoining = false;

/*! Collector's address */
static ApiMac_sAddr_t collectorAddr = {0};

/* Join Time Ticks (used for average join time calculations) */
static uint_fast32_t joinTimeTicks = 0;

/* End to end delay statistics timestamp */
static uint32_t startSensorMsgTimeStamp = 0;

/*! Device's Outgoing MSDU Handle values */
STATIC uint8_t deviceTxMsduHandle = 0;

#ifdef IEEE_COEX_TEST
Smsgs_configReqMsg_t configSettings;
#else
STATIC Smsgs_configReqMsg_t configSettings;
#endif

#if !defined(OAD_IMG_A) && !defined(POWER_MEAS)
/*!
    振动传感器字段 - 仅当frameControl中设置了Smsgs_dataFields_accelSingleAxisSensor,Smsgs_dataFields_accelThreeAxisSensor时有效。
 */
STATIC Smsgs_vibrateSensorField_t vibrateSensor = { 0 };
/*!
    vibrate Template and Battery field - valid only if Smsgs_dataFields_tempAndBattery
    is set in frameControl.
    */
//STATIC Smsgs_vibrateTemplateBat_t vibrateTemplateBat = {0};
/*!
    vibrate Sample Config field - valid only if Smsgs_dataFields_samplingConfig
    is set in frameControl.
    */
STATIC Smsgs_vibrateSampleConfig_t vibrateSampleConfig = {0};
/*!
    vibrate Channel Config field - valid only if Smsgs_dataFields_channelConfig
    is set in frameControl.
    */
STATIC Smsgs_vibrateChannelConfig_t vibrateChannelConfig[SENSOR_CHANNEL_NUM] = {0};
/*!
    vibrate Channel Temp Config field - valid only if Smsgs_dataFields_channelTempConfig
    is set in frameControl.
    */
static Smsgs_vibrateChannelTempConfig_t vibrateChannelTempConfig= {0};

#endif /* !defined(OAD_IMG_A) && !defined(POWER_MEAS) */

STATIC Llc_netInfo_t parentInfo = {0};

STATIC uint16_t lastRcvdBroadcastMsgId = 0;

#ifdef FEATURE_SECURE_COMMISSIONING
/* variable to store the current setting of auto Request Pib attribute
 * before it gets modified by SM module, in beacon mode
 */
static bool currAutoReq = 0;
SMMsgs_authMethod_t smAuthMethod = SM_SENSOR_DEFAULT_AUTH_METHOD;
#endif /* FEATURE_SECURE_COMMISSIONING */

#ifdef DMM_OAD
ApiMac_deviceDescriptor_t cacheddevInfo = {0};
Llc_netInfo_t cachedparentInfo = {0};
#endif

#if defined(DISPLAY_PER_STATS)
extern void Ssf_displayRxStats(int8_t rssi);
#endif

/* Reboot timer and context */
static ClockP_Struct rebootClkStruct;
static ClockP_Handle rebootClkHandle = NULL;
static bool rebootTimerInitialized = false;

/* Reboot delay limits in milliseconds */
#define SENSOR_REBOOT_DELAY_MIN_MS   10U
#define SENSOR_REBOOT_DELAY_MAX_MS   60000U


// 控制每次发送的帧控制位，避免所有数据一起发送，因为包大小最多100字节
// 较小的数据包可以一起发送，较大的数据包需要分次发送
// static uint16_t frameControlMask[] = {
//     0x0018,//(Smsgs_dataFields_msgStats | Smsgs_dataFields_configSettings),
//     Smsgs_dataFields_samplingConfig,
//     Smsgs_dataFields_channelConfig,
//     Smsgs_dataFields_channelTempConfig,
// };
// #define FRAME_CONTROL_MASK_LEN (sizeof(frameControlMask) / sizeof(frameControlMask[0]))

Display_Handle display;

/******************************************************************************
 Local function prototypes
 *****************************************************************************/
static void initializeClocks(void);
static void commStatusIndCB(ApiMac_mlmeCommStatusInd_t *pCommStatusInd);
static void dataCnfCB(ApiMac_mcpsDataCnf_t *pDataCnf);
static void dataIndCB(ApiMac_mcpsDataInd_t *pDataInd);
static uint8_t getMsduHandle(Smsgs_cmdIds_t msgType);

#if !defined(OAD_IMG_A) && !defined(POWER_MEAS)
static bool processSensorMsgEvt(uint16_t frameControl);
static bool sendSensorMessage(ApiMac_sAddr_t *pDstAddr,
                              Smsgs_sensorMsg_t *pMsg);
static void readSensors(void);

#endif /* !defined(OAD_IMG_A) && !defined(POWER_MEAS) */

#if SENSOR_TEST_RAMP_DATA_SIZE && (CERTIFICATION_TEST_MODE || defined(POWER_MEAS))
static void processSensorRampMsgEvt(void);
#endif

static void processConfigRequest(ApiMac_mcpsDataInd_t *pDataInd);
static void processBroadcastCtrlMsg(ApiMac_mcpsDataInd_t *pDataInd);
static void processSensorDataMessage(ApiMac_mcpsDataInd_t *pDataInd);
static void readSavedConfigurationData(void);
static bool sendConfigRsp(ApiMac_sAddr_t *pDstAddr, Smsgs_configRspMsg_t *pMsg);
static uint16_t validateFrameControl(uint16_t frameControl);

#if defined(DEVICE_TYPE_MSG)
static void Sensor_sendDeviceTypeResponse(void);
#endif /* DEVICE_TYPE_MSG */

static void jdllcJoinedCb(ApiMac_deviceDescriptor_t *pDevInfo,
                          Llc_netInfo_t  *pStartedInfo);
static void jdllcDisassocIndCb(ApiMac_sAddrExt_t *extAddress,
                               ApiMac_disassocateReason_t reason);
static void jdllcDisassocCnfCb(ApiMac_sAddrExt_t *extAddress,
                               ApiMac_status_t status);
static void jdllcStateChangeCb(Jdllc_states_t state);

#ifdef FEATURE_SECURE_COMMISSIONING
/* Security Manager callback functions */
static void smFailCMProcessCb(ApiMac_deviceDescriptor_t *devInfo,
                              bool rxOnIdle, bool keyRefreshment,
                              SMMsgs_errorCode_t errorCode);
static void smSuccessCMProcessCb(ApiMac_deviceDescriptor_t *devInfo,
                                 bool keyRefreshment);
#endif /* FEATURE_SECURE_COMMISSIONING */

#ifdef DMM_OAD
static void Sensor_dmmPausePolicyCb(uint16_t pause);
#endif /* DMM_OAD */

// static void processRampResp(ApiMac_mcpsDataInd_t *pDataInd);
static int_fast16_t Sensor_postNotifyFxn_entering_standby(unsigned int eventType, uintptr_t eventArg, uintptr_t clientArg);
static int_fast16_t Sensor_postNotifyFxn_awake_standby(unsigned int eventType, uintptr_t eventArg, uintptr_t clientArg);

static void processRebootTimeoutCallback(uintptr_t a0);
static void Sensor_scheduleReboot(uint16_t delayMs);

/******************************************************************************
 Callback tables
 *****************************************************************************/

/*! API MAC Callback table */
STATIC ApiMac_callbacks_t Sensor_macCallbacks =
    {
      /*! Associate Indicated callback */
      NULL,
      /*! Associate Confirmation callback */
      NULL,
      /*! Disassociate Indication callback */
      NULL,
      /*! Disassociate Confirmation callback */
      NULL,
      /*! Beacon Notify Indication callback */
      NULL,
      /*! Orphan Indication callback */
      NULL,
      /*! Scan Confirmation callback */
      NULL,
      /*! Start Confirmation callback */
      NULL,
      /*! Sync Loss Indication callback */
      NULL,
      /*! Poll Confirm callback */
      NULL,
      /*! Comm Status Indication callback */
      commStatusIndCB,
      /*! Poll Indication Callback */
      NULL,
      /*! Data Confirmation callback */
      dataCnfCB,
      /*! Data Indication callback */
      dataIndCB,
      /*! Purge Confirm callback */
      NULL,
      /*! WiSUN Async Indication callback */
      NULL,
      /*! WiSUN Async Confirmation callback */
      NULL,
      /*! Unprocessed message callback */
      NULL
    };

STATIC Jdllc_callbacks_t jdllcCallbacks =
    {
      /*! Network Joined Indication callback */
      jdllcJoinedCb,
      /* Disassociation Indication callback */
      jdllcDisassocIndCb,
      /* Disassociation Confirm callback */
      jdllcDisassocCnfCb,
      /*! State Changed indication callback */
      jdllcStateChangeCb
    };
#ifdef FEATURE_SECURE_COMMISSIONING
STATIC SM_callbacks_t SMCallbacks =
    {
      /*! Request passkey callback */
      Ssf_SmPasskeyEntry,
      /*! Security authentication failed callback */
      smFailCMProcessCb,
      /* Security authentication successful callback */
      smSuccessCMProcessCb
    };
#endif /* FEATURE_SECURE_COMMISSIONING */

#ifdef DMM_OAD
/*********************************************************************
 * DMM Policy Callbacks
 */
static DMMPolicy_AppCbs_t dmmPolicyAppCBs =
{
     Sensor_dmmPausePolicyCb
};
#endif
/******************************************************************************
 Public Functions
 *****************************************************************************/

/*!
 * @brief 获取系统配置参数句柄
 */
const Smsgs_configReqMsg_t* Sensor_getConfigSettings(void)
{
    return &configSettings;
}

/*!
 * @brief 获取振动传感器字段句柄
 */
Smsgs_vibrateSensorField_t* Sensor_getVibrateSensorField(void)
{
    return &vibrateSensor;
}

/*!
 * @brief 获取振动温度和电池字段句柄
 */
/*Smsgs_vibrateTemplateBat_t* Sensor_getVibrateTemplateBat(void)
{
    return &vibrateTemplateBat;
}*/

/*!
 * @brief 获取振动采样配置字段句柄
 */
Smsgs_vibrateSampleConfig_t* Sensor_getVibrateSampleConfig(void)
{
    return &vibrateSampleConfig;
}

/*!
 * @brief 获取振动通道配置字段句柄
 */
Smsgs_vibrateChannelConfig_t* Sensor_getVibrateChannelConfig(void)
{
    return vibrateChannelConfig;
}

/*!
 * @brief 获取振动通道温度配置字段句柄
 */
Smsgs_vibrateChannelTempConfig_t* Sensor_getVibrateChannelTempConfig(void)
{
    return &vibrateChannelTempConfig;
}

/*!
 * @brief 将frameControl添加到发送队列
 * 
 * @param frameControl - 要发送的frameControl
 * 
 * @return true - 成功入队
 *         false - 入队失败（队列已满）
 */
bool Sensor_enqueueFrameControl(uint16_t frameControl)
{
    bool ret = FrameControlQueue_enqueue(frameControl);
    if (ret)
    {
        DEBUG_PRINT("Enqueued frameControl: 0x%04x", frameControl);
        // 触发发送事件
        Ssf_setReadingClock(20);
    }
    else
    {
        DEBUG_PRINT("Failed to enqueue frameControl: 0x%04x (Queue full)", frameControl);
    }
    return ret;
}

/*!
 * @brief 批量将frameControl添加到发送队列
 * 
 * @param pFrameControls - frameControl数组
 * @param count - 数组元素数量
 * 
 * @return 实际成功入队的数量
 */
uint16_t Sensor_enqueueFrameControlBatch(const uint16_t *pFrameControls, uint16_t count)
{
    uint16_t enqueuedCount = FrameControlQueue_enqueueBatch(pFrameControls, count);
    
    if (enqueuedCount > 0)
    {
        DEBUG_PRINT("Enqueued %d frameControls", enqueuedCount);
        // 触发发送事件
        Ssf_setReadingClock(20);
    }
    
    return enqueuedCount;
}


/*!
 * @brief 载入默认发送数据类型到队列
 */
void Sensor_loadDefaultFrameControl(void)
{
    // 载入默认发送数据类型到队列
    // Sensor_enqueueFrameControl(Smsgs_dataFields_accelSingleAxisSensor);
    // Sensor_enqueueFrameControl(Smsgs_dataFields_accelThreeAxisSensor);
    Sensor_enqueueFrameControl(Smsgs_dataFields_samplingConfig);
    Sensor_enqueueFrameControl(Smsgs_dataFields_channelConfig);
    Sensor_enqueueFrameControl(Smsgs_dataFields_channelTempConfig);
    Sensor_enqueueFrameControl(Smsgs_dataFields_configSettings);
}

/*!
 Initialize this application.

 Public function defined in sensor.h
 */
#ifdef OSAL_PORT2TIRTOS
void Sensor_init(uint8_t macTaskId)
#else
void Sensor_init(void)
#endif
{
    ApiMac_deviceDescriptor_t devInfo;
    Llc_netInfo_t parentInfo;
    uint32_t frameCounter = 0;
    /* Initialize the sensor's structures */
    memset(&configSettings, 0, sizeof(Smsgs_configReqMsg_t));
#if defined(VIBRATE_SENSOR)
    // 设置允许发送的数据类型
    configSettings.frameControl |= Smsgs_dataFields_tempAndBattery;
    configSettings.frameControl |= Smsgs_dataFields_samplingConfig;
    configSettings.frameControl |= Smsgs_dataFields_channelConfig;
    configSettings.frameControl |= Smsgs_dataFields_channelTempConfig;
    configSettings.frameControl |= Smsgs_dataFields_configSettings;
#endif
    configSettings.frameControl |= Smsgs_dataFields_msgStats;

    if(!CERTIFICATION_TEST_MODE)
    {
        configSettings.reportingInterval = CONFIG_REPORTING_INTERVAL;
    }
    else
    {
        /* start back to back data transmission at the earliest */
        configSettings.reportingInterval = 100;
    }
    configSettings.pollingInterval = CONFIG_POLLING_INTERVAL;

    /* Initialize the MAC */
#ifdef OSAL_PORT2TIRTOS
    sem = ApiMac_init(macTaskId, CONFIG_FH_ENABLE);
#else
    sem = ApiMac_init(CONFIG_FH_ENABLE);
#endif

    /* Initialize the Joining Device Logical Link Controller */
    Jdllc_init(&Sensor_macCallbacks, &jdllcCallbacks);

    /* Register the MAC Callbacks */
    ApiMac_registerCallbacks(&Sensor_macCallbacks);

    /* Initialize the platform specific functions */
    Ssf_init(sem);

#ifdef FEATURE_SECURE_COMMISSIONING
    /* Intialize the security manager and register callbacks */
    SM_registerCallback(&SMCallbacks);
#endif /* FEATURE_SECURE_COMMISSIONING */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_phyCurrentDescriptorId,
                           (uint8_t)CONFIG_PHY_ID);

    ApiMac_mlmeSetReqUint8(ApiMac_attribute_channelPage,
                           (uint8_t)CONFIG_CHANNEL_PAGE);
    Ssf_getFrameCounter(NULL, &frameCounter);

#ifdef FEATURE_MAC_SECURITY
    /* Initialize the MAC Security */
    Jdllc_securityInit(frameCounter, NULL);
#endif /* FEATURE_MAC_SECURITY */

    /* Set the transmit power */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_phyTransmitPowerSigned,
                           (uint8_t)CONFIG_TRANSMIT_POWER);
    /* Set Min BE */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_backoffExponent,
                              (uint8_t)CONFIG_MIN_BE);
    /* Set Max BE */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_maxBackoffExponent,
                              (uint8_t)CONFIG_MAX_BE);
    /* Set MAC MAX CSMA Backoffs */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_maxCsmaBackoffs,
                              (uint8_t)CONFIG_MAC_MAX_CSMA_BACKOFFS);
    /* Set MAC MAX Frame Retries */
    ApiMac_mlmeSetReqUint8(ApiMac_attribute_maxFrameRetries,
                              (uint8_t)CONFIG_MAX_RETRIES);
#ifdef FCS_TYPE16
    /* Set the fcs type */
    ApiMac_mlmeSetReqBool(ApiMac_attribute_fcsType,
                           (bool)1);
#endif

#ifdef MAC_DUTY_CYCLE_CHECKING
    ApiMac_mlmeSetReqBool(ApiMac_attribute_dutyCycleEnabled, true);
    ApiMac_mlmeSetReqUint32(ApiMac_attribute_dutyCycleRegulated,
                            DUTY_CYCLE_MEAS_PERIOD*MAC_DUTY_CYCLE_THRESHOLD/100);

    /* Critical and limited duty cycle modes unused, set to max
     * value to avoid entering state */
    ApiMac_mlmeSetReqUint32(ApiMac_attribute_dutyCycleCritical,
                            UINT32_MAX);
    ApiMac_mlmeSetReqUint32(ApiMac_attribute_dutyCycleLimited,
                            UINT32_MAX);
#endif

#ifdef MAC_OVERRIDE_TX_DELAY
    ApiMac_mlmeSetReqBool(ApiMac_attribute_customMinTxOffEnabled, true);
    ApiMac_mlmeSetReqUint32(ApiMac_attribute_minTxOffTime,
                            (uint32_t)MAC_CONFIG_MIN_TX_OFF);
#endif

    /* Initialize the app clocks */
    initializeClocks();

    /* Initialize the new modules */
    PacketTransmission_init();
    
    /* Initialize frame control queue */
    if (!FrameControlQueue_init())
    {
        DEBUG_PRINT("Error: Failed to initialize frame control queue");
    }

    if(Ssf_getNetworkInfo(&devInfo, &parentInfo) == true)
    {
        /* Update Channel Mask to show the previous network channel */
        if (!CONFIG_FH_ENABLE)
        {
            uint8_t channelMask[APIMAC_154G_CHANNEL_BITMAP_SIZ] = {0};
            uint8_t idx = parentInfo.channel / 8;
            uint8_t shift = (parentInfo.channel % 8);
            uint8_t chan = (0x01) << shift;
            channelMask[idx] = chan;
            Jdllc_setChanMask(channelMask);
        }
        /* Start the device */
        Util_setEvent(&Sensor_events, SENSOR_START_EVT);
    }
    else if (CONFIG_AUTO_START)
    {
        /* Start the device */
        Util_setEvent(&Sensor_events, SENSOR_START_EVT);
    }

#ifdef DMM_OAD
    // register the app callbacks
    DMMPolicy_registerAppCbs(dmmPolicyAppCBs, DMMPolicy_StackRole_154Sensor);
#endif
    /* Open the UART display for output */
    display = Display_open(Display_Type_UART, NULL);
    if (display == NULL)
    {
        while (1) {}
    }

    // FOR TEST RAW TX
    // PacketTransmission_startRawTx(1000000, g_rawDataBuffer, 70);
    // PacketTransmission_startRawTx(2000000, g_rawDataBuffer, 50);

    // 填充传感器数据
    // SensorDataGenerator_generateSensorData(&vibrateSensor);
    // SensorDataGenerator_generateTemplateBatData(&vibrateTemplateBat);
    // SensorDataGenerator_generateSampleConfigData(&vibrateSampleConfig);
    // SensorDataGenerator_generateVibrateConfigData(&vibrateChannelConfig[0], 0);
    // SensorDataGenerator_generateVibrateConfigData(&vibrateChannelConfig[1], 1);
    // SensorDataGenerator_generateVibrateConfigData(&vibrateChannelConfig[2], 2);
    // SensorDataGenerator_generateVibrateTemplateConfigData(&vibrateChannelTempConfig, 0);
    // SensorDataGenerator_generateVibrateTemplateConfigData(&vibrateChannelTempConfig[1], 1);
    // SensorDataGenerator_generateVibrateTemplateConfigData(&vibrateChannelTempConfig[2], 2);

    /* 读取保存的配置数据 */
    readSavedConfigurationData();

    /* Register notification function */
    static Power_NotifyObj postNotify_entering_standby;
    static Power_NotifyObj postNotify_awake_standby;
    Power_registerNotify(&postNotify_entering_standby, PowerCC26XX_ENTERING_STANDBY, Sensor_postNotifyFxn_entering_standby, (uintptr_t)NULL);
    Power_registerNotify(&postNotify_awake_standby, PowerCC26XX_AWAKE_STANDBY, Sensor_postNotifyFxn_awake_standby, (uintptr_t)NULL);

    DEBUG_PRINT("Sensor Initialized");
}


ApiMac_sAddr_t *getCollectorAddr(void)
{
    return &collectorAddr;
}

/*!
 Application task processing.

 Public function defined in sensor.h
 */
void Sensor_process(void)
{
    /* Start the collector device in the network */
    if(Sensor_events & SENSOR_START_EVT)
    {
        ApiMac_deviceDescriptor_t devInfo;
        Llc_netInfo_t parentInfo;

        if(Ssf_getNetworkInfo(&devInfo, &parentInfo) == true)
        {
            rejoining = true;

            Ssf_configSettings_t configInfo;
#ifdef FEATURE_MAC_SECURITY
            ApiMac_status_t stat;
#endif /* FEATURE_MAC_SECURITY */

            /* Do we have config settings? */
            if(Ssf_getConfigInfo(&configInfo) == true)
            {
                /* Save the config information */
                configSettings.frameControl = configInfo.frameControl;
                configSettings.reportingInterval = configInfo.reportingInterval;
                configSettings.pollingInterval = configInfo.pollingInterval;
                DEBUG_PRINT("Loading saved config: frameControl = 0x%04x, reportingInterval = %d, pollingInterval = %d", 
                               configInfo.frameControl, configInfo.reportingInterval, configInfo.pollingInterval);
                
                /* Update the polling interval in the LLC */
                Jdllc_setPollRate(configSettings.pollingInterval);
            }

            /* Initially, setup the parent as the collector */
            if(parentInfo.fh == true && CONFIG_RX_ON_IDLE)
            {
                collectorAddr.addrMode = ApiMac_addrType_extended;
                memcpy(&collectorAddr.addr.extAddr,
                       parentInfo.devInfo.extAddress, APIMAC_SADDR_EXT_LEN);
            }
            else
            {
                collectorAddr.addrMode = ApiMac_addrType_short;
                collectorAddr.addr.shortAddr = parentInfo.devInfo.shortAddress;
            }

#ifdef FEATURE_MAC_SECURITY
            /* Put the parent in the security device list */
            stat = Jdllc_addSecDevice(parentInfo.devInfo.panID,
                                      parentInfo.devInfo.shortAddress,
                                      &parentInfo.devInfo.extAddress, 0);
            if(stat != ApiMac_status_success)
            {
                Ssf_displayError("Auth Error: 0x", (uint8_t)stat);
            }
#endif /* FEATURE_MAC_SECURITY */

#ifdef FEATURE_SECURE_COMMISSIONING
            if(!CONFIG_FH_ENABLE)
            {
                nvDeviceKeyInfo_t devKeyInfo;
                if(Ssf_getDeviceKeyInfo(&devKeyInfo) == TRUE)
                {
                    SM_recoverKeyInfo(devInfo, parentInfo, devKeyInfo);
                }

            }
#endif /* FEATURE_SECURE_COMMISSIONING */
            Jdllc_rejoin(&devInfo, &parentInfo);
        }
        else
        {
            /* Reset flag when joining a new network */
            rejoining = false;

            /* Get Start Timestamp */
#ifdef OSAL_PORT2TIRTOS
            joinTimeTicks = ClockP_getSystemTicks();
#else
            joinTimeTicks = ICall_getTicks();
#endif
            Jdllc_join();
        }

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_START_EVT);
    }


    /* Is it time to send the next sensor data message? */
    if(Sensor_events & SENSOR_READING_TIMEOUT_EVT)
    {

#if !defined(OAD_IMG_A)

        /* In certification test mode, back to back data shall be sent */
        if(!CERTIFICATION_TEST_MODE)
        {
            /* Setup for the next message */
            Ssf_setReadingClock(configSettings.reportingInterval);
        }

#ifdef FEATURE_SECURE_COMMISSIONING
        /* if secure Commissioning feature is enabled, read
         * sensor data and send it only after the secure
         * commissioning process is done successfully.
         * else, do not read and send sensor data.
         */
        if(SM_Current_State != SM_CM_InProgress)
        {
#endif /* FEATURE_SECURE_COMMISSIONING */


#if SENSOR_TEST_RAMP_DATA_SIZE && (CERTIFICATION_TEST_MODE || defined(POWER_MEAS))
        processSensorRampMsgEvt();
#endif /* SENSOR_TEST_RAMP_DATA_SIZE */

#if !defined(POWER_MEAS)
        /* Read sensors */
        readSensors();
        
        /* 从队列中读取所有数据并发送 */
        uint16_t frameControl;
        uint16_t processedCount = 0;
        bool sendFailed = false;
        
        // 处理队列中的所有数据
        while (FrameControlQueue_dequeue(&frameControl))
        {
            if(!(configSettings.frameControl & frameControl))
            {
                DEBUG_PRINT("FrameControl not allowed: 0x%04x", frameControl);
                continue;
            }
            /* Process Sensor Reading Message Event */
            if (processSensorMsgEvt(frameControl))
            {
                // 发送成功
                processedCount++;
                DEBUG_PRINT("Send Success: frameControl = 0x%04x", frameControl);
            }
            else
            {
                // 发送失败，重新放回队列
                FrameControlQueue_enqueue(frameControl);
                DEBUG_PRINT("Send Failed: frameControl = 0x%04x, retry later", frameControl);
                sendFailed = true;
                break; // 发送失败，停止继续发送
            }
        }
        
        // 根据处理结果设置下次定时器
        if (sendFailed)
        {
            // 有发送失败的，1秒后重试
            Ssf_setReadingClock(1000);
        }
        else if (processedCount > 0)
        {
            // 所有数据发送成功
            DEBUG_PRINT("Send Complete: %d items sent\n", processedCount);
            
            // 检查队列是否还有数据（可能在发送过程中有新数据入队）
            if (!FrameControlQueue_isEmpty())
            {
                Ssf_setReadingClock(50); // 50ms后继续发送
            }
        }

        // 发送需要周期上报的数据
        // {
        //     // 队列为空，填充队列
        //     uint8_t i;
        //     uint16_t enqueuedCount = 0;
        //     for (i = 0; i < FRAME_CONTROL_MASK_LEN; i++)
        //     {
        //         uint16_t fc = frameControlMask[i] & configSettings.frameControl;
        //         if (fc)
        //         {
        //             if (FrameControlQueue_enqueue(fc))
        //             {
        //                 enqueuedCount++;
        //             }
        //             else
        //             {
        //                 DEBUG_PRINT("Warning: Queue full, stopped at index %d", i);
        //                 break;
        //             }
        //         }
        //     }
            
        //     if (enqueuedCount > 0)
        //     {
        //         DEBUG_PRINT("Enqueued %d frame controls", enqueuedCount);
        //         Ssf_setReadingClock(configSettings.reportingInterval); // 10ms后开始发送
        //     }
        // }
        
#endif /* POWER_MEAS */

#ifdef FEATURE_SECURE_COMMISSIONING
        }
#endif /* FEATURE_SECURE_COMMISSIONING */

#endif //OAD_IMG_A

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_READING_TIMEOUT_EVT);
    }

    if(Sensor_events & SENSOR_SEND_RAW_DATA_EVT) {
        PacketTransmission_processSensorRawTxMsgEvt();
    }

    /* Reboot event: perform system reset */
    if(Sensor_events & SENSOR_REBOOT_EVT)
    {
        /* Clear the event first */
        Util_clearEvent(&Sensor_events, SENSOR_REBOOT_EVT);

        /* Perform system reset; this call should not return */
        SysCtrlSystemReset();
    }

#if defined(OAD_IMG_A)
    if(Sensor_events & SENSOR_OAD_SEND_RESET_RSP_EVT)
    {
        OADClient_processEvent(&Sensor_events);
    }
#endif //OAD_IMG_A

#ifdef DISPLAY_PER_STATS
    /* Is it time to update the PER display? */
    if(Sensor_events & SENSOR_UPDATE_STATS_EVT)
    {
        Ssf_displayPerStats(&Sensor_msgStats);

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_UPDATE_STATS_EVT);
   }
#endif /* DISPLAY_PER_STATS */

    /* Is it disassociate event? */
    if(Sensor_events & SENSOR_DISASSOC_EVT)
    {
        Jdllc_sendDisassociationRequest();

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_DISASSOC_EVT);
    }

// #ifdef DMM_OAD
    if(Sensor_events & SENSOR_PAUSE_EVT)
    {
        /* Turn off timers to temporarily stop transmissions */
        Ssf_setPollClock(0);
        Ssf_setReadingClock(0);
        if(CONFIG_FH_ENABLE)
        {
            Ssf_setTrickleClock(0, ApiMac_wisunAsyncFrame_advertisementSolicit);
            Ssf_setTrickleClock(0, ApiMac_wisunAsyncFrame_configSolicit);
        }

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_PAUSE_EVT);
    }
    if(Sensor_events & SENSOR_RESUME_EVT)
    {
        /* Call SENSOR_START_EVT to re-associate with collector if BLE-OAD fails */
        Util_setEvent(&Sensor_events, SENSOR_START_EVT);

        /* Wake up the application thread when it waits for clock event */
        Ssf_PostAppSem();

        /* Clear the event */
        Util_clearEvent(&Sensor_events, SENSOR_RESUME_EVT);
    }
// #endif /* DMM_OAD */

    /* Process LLC Events */
    Jdllc_process();

    /* Allow the Specific functions to process */
    Ssf_processEvents();

#ifdef FEATURE_SECURE_COMMISSIONING
    /* Allow the security manager specific functions to process */
    SM_process();
#endif /* FEATURE_SECURE_COMMISSIONING */
    /*
     Don't process ApiMac messages until all of the sensor events
     are processed.
     */
#ifdef FEATURE_SECURE_COMMISSIONING
    /*only if there are no sensor events and security manager events to handle*/
    if((Sensor_events == 0) && (SM_events == 0))
#else
    if(Sensor_events == 0)
#endif /* FEATURE_SECURE_COMMISSIONING */
    {
        /* Wait for response message or events */
        ApiMac_processIncoming();
    }
}
#ifdef TX_POWER_TEST
uint32_t txPowerLimitTest = 0;
uint8_t txTestPower1 = 0;
uint8_t txTestPower2 = 5;
#endif

#if defined(IEEE_COEX_TEST) && defined(COEX_MENU)
extern uint32_t numPacket;
extern uint32_t nSensorPkts;
#endif
/*!
 * @brief   Send MAC data request
 *
 * @param   type - message type
 * @param   pDstAddr - destination address
 * @param   rxOnIdle - true if not a sleepy device
 * @param   len - length of payload
 * @param   pData - pointer to the buffer
 *
 * @return  true if sent, false if not
 */
bool Sensor_sendMsg(Smsgs_cmdIds_t type, ApiMac_sAddr_t *pDstAddr,
                    bool rxOnIdle, uint16_t len, uint8_t *pData)
{
    bool ret = false;
    /* information about the network */
    ApiMac_mcpsDataReq_t dataReq;

#if defined(IEEE_COEX_TEST) && defined(COEX_MENU)
    if(nSensorPkts >= numPacket)
    {
        return (true);
    }
    nSensorPkts++;
#endif

    /* Timestamp to compute end to end delay */
#ifdef OSAL_PORT2TIRTOS
    startSensorMsgTimeStamp = ClockP_getSystemTicks();
#else
    startSensorMsgTimeStamp = ICall_getTicks();
#endif

    /* Construct the data request field */
    memset(&dataReq, 0, sizeof(ApiMac_mcpsDataReq_t));
    memcpy(&dataReq.dstAddr, pDstAddr, sizeof(ApiMac_sAddr_t));

    /* set the correct address mode. */
    if(pDstAddr->addrMode == ApiMac_addrType_extended)
    {
        dataReq.srcAddrMode = ApiMac_addrType_extended;
    }
    else
    {
        dataReq.srcAddrMode = ApiMac_addrType_short;
    }

    if(rejoining == true)
    {
        /* get the new panID from the mac */
        ApiMac_mlmeGetReqUint16(ApiMac_attribute_panId,
                                &(parentInfo.devInfo.panID));
    }

    dataReq.dstPanId = parentInfo.devInfo.panID;

    dataReq.msduHandle = getMsduHandle(type);

    dataReq.txOptions.ack = true;

    if(type == Smsgs_cmdIds_rampdata)
    {
        dataReq.txOptions.ack = false;
    }
    
    if(CERTIFICATION_TEST_MODE)
    {
        dataReq.txOptions.ack = false;
    }

    if(rxOnIdle == false)
    {
        dataReq.txOptions.indirect = true;
    }

    dataReq.msdu.len = len;
    dataReq.msdu.p = pData;

#ifdef FEATURE_MAC_SECURITY
#ifdef FEATURE_SECURE_COMMISSIONING
    {
        extern ApiMac_sAddrExt_t ApiMac_extAddr;
        SM_getSrcDeviceSecurityInfo(ApiMac_extAddr, SM_Sensor_SAddress, &dataReq.sec);
    }
#else
    Jdllc_securityFill(&dataReq.sec);
#endif /* FEATURE_SECURE_COMMISSIONING */
#endif /* FEATURE_MAC_SECURITY */

    if(type == Smsgs_cmdIds_sensorData || type == Smsgs_cmdIds_rampdata)
    {
        Sensor_msgStats.msgsAttempted++;
    }
    else if(type == Smsgs_cmdIds_trackingRsp)
    {
        Sensor_msgStats.trackingResponseAttempts++;
    }
    else if(type == Smsgs_cmdIds_configRsp)
    {
        Sensor_msgStats.configResponseAttempts++;
    }
#ifdef TX_POWER_TEST
    if (txPowerLimitTest % 2 == 0)
    {
        dataReq.power = txTestPower1;
    }
    else
    {
        dataReq.power = txTestPower2;
    }
#endif
    /* Send the message */
    if(ApiMac_mcpsDataReq(&dataReq) == ApiMac_status_success)
    {
        ret = true;
    }
    else
    {
        /* handle transaction overflow by retrying */
        if(type == Smsgs_cmdIds_rampdata)
        {
            // DEBUG_PRINT("send failed");
        }
        else if(type == Smsgs_cmdIds_sensorData)
        {
            
            Ssf_setReadingClock(configSettings.reportingInterval);
        }
    }
#ifdef TX_POWER_TEST
    txPowerLimitTest++;
#endif
    return (ret);
}


/*!
 Send LED Identify Request to collector

 Public function defined in sensor.h
 */
void Sensor_sendIdentifyLedRequest(void)
{
    DataSender_sendIdentifyLedRequest(&collectorAddr);
}

#ifdef FEATURE_SECURE_COMMISSIONING
/*!
 * @brief Sets the Security Authentication Mode
 *
 *  Public function defined in sensor.h
 */
extern void Sensor_setSmAuthMethod(SMMsgs_authMethod_t authMethod)
{
    smAuthMethod = authMethod;
}

/*!
 * @brief Gets the Security Authentication Mode
 *
 *  Public function defined in sensor.h
 */
extern SMMsgs_authMethod_t Sensor_getSmAuthMethod(void)
{
    return smAuthMethod;
}
#endif /* FEATURE_SECURE_COMMISSIONING */

/******************************************************************************
 Local Functions
 *****************************************************************************/

/*!
 * @brief       Initialize the clocks.
 *
 */
static void initializeClocks(void)
{
    /* Initialize the reading clock */
    Ssf_initializeReadingClock();
}

/*!
 * @brief       Reboot timeout callback.
 *
 *              This runs in timer context; only post an event and wake the app.
 */
static void processRebootTimeoutCallback(uintptr_t a0)
{
    (void)a0;

    Util_setEvent(&Sensor_events, SENSOR_REBOOT_EVT);
    /* Wake up the application thread when it waits for clock event */
    Ssf_PostAppSem();
}

/*!
 * @brief       Schedule a delayed reboot based on RebootCmd.
 *
 * @param       delayMs  - requested delay in milliseconds
 */
static void Sensor_scheduleReboot(uint16_t delayMs)
{
    uint32_t timeoutMs = delayMs;

    /* Normalize delay range */
    if(timeoutMs < SENSOR_REBOOT_DELAY_MIN_MS)
    {
        timeoutMs = SENSOR_REBOOT_DELAY_MIN_MS;
    }
    else if(timeoutMs > SENSOR_REBOOT_DELAY_MAX_MS)
    {
        timeoutMs = SENSOR_REBOOT_DELAY_MAX_MS;
    }

    /* Lazy initialize reboot timer */
    if(!rebootTimerInitialized)
    {
        rebootClkHandle = UtilTimer_construct(&rebootClkStruct,
                                              processRebootTimeoutCallback,
                                              timeoutMs,
                                              0,
                                              false,
                                              0);
        rebootTimerInitialized = (rebootClkHandle != NULL);
    }

    if(!rebootTimerInitialized)
    {
        /* If timer cannot be created, fall back to immediate reboot */
        Util_setEvent(&Sensor_events, SENSOR_REBOOT_EVT);
        Ssf_PostAppSem();
        return;
    }

    /* Stop if running */
    if(UtilTimer_isActive(&rebootClkStruct) == true)
    {
        UtilTimer_stop(&rebootClkStruct);
    }

    /* Configure and start timer */
    UtilTimer_setTimeout(rebootClkHandle, timeoutMs);
    UtilTimer_start(&rebootClkStruct);
}

/*!
 * @brief      Process the MAC Comm Status Indication Callback
 *
 * @param      pCommStatusInd - Comm Status indication
 */
static void commStatusIndCB(ApiMac_mlmeCommStatusInd_t *pCommStatusInd)
{
    uint32_t stat;

    /* Frame received with security error */
    if (pCommStatusInd->reason == ApiMac_commStatusReason_rxSecure)
    {

        ApiMac_mlmeGetReqUint32(ApiMac_attribute_diagRxSecureFail, &stat);
        Sensor_msgStats.rxDecryptFailures = (uint16_t)stat;

#ifndef MAC_OVERRIDE_TX_DELAY
        /* Enabling the minimum custom TX off-time feature in SysConfig
         * will overwrite ApiMac_attribute_diagTxSecureFail MAC PIB. See
         * api_mac.c/h
         */
        ApiMac_mlmeGetReqUint32(ApiMac_attribute_diagTxSecureFail, &stat);
        Sensor_msgStats.txEncryptFailures = (uint16_t)stat;
#endif

        Ssf_displayError("Security Error: ", pCommStatusInd->status);
    }
}

/*!
 * @brief      MAC Data Confirm callback.
 *
 * @param      pDataCnf - pointer to the data confirm information
 */
static void dataCnfCB(ApiMac_mcpsDataCnf_t *pDataCnf)
{

    /* Record statistics */
    if(pDataCnf->status == ApiMac_status_channelAccessFailure)
    {
        Sensor_msgStats.channelAccessFailures++;
    }
    else if(pDataCnf->status == ApiMac_status_noAck)
    {
        Sensor_msgStats.macAckFailures++;
#ifdef DISPLAY_PER_STATS
        Util_setEvent(&Sensor_events, SENSOR_UPDATE_STATS_EVT);
#endif /* DISPLAY_PER_STATS */

    }
    else if(pDataCnf->status != ApiMac_status_success)
    {
        Sensor_msgStats.otherDataRequestFailures++;
#ifdef DISPLAY_PER_STATS
        Util_setEvent(&Sensor_events, SENSOR_UPDATE_STATS_EVT);
#endif /* DISPLAY_PER_STATS */

        Ssf_displayError("dataCnf: ", pDataCnf->status);
    }
    else if(pDataCnf->status == ApiMac_status_success)
    {
        Ssf_updateFrameCounter(NULL, pDataCnf->frameCntr);
    }

#ifdef FEATURE_SECURE_COMMISSIONING
    /* Strictly ensure that the message did not come from the app */
    if(!(pDataCnf->msduHandle & APP_MARKER_MSDU_HANDLE))
    {
        if ((SM_CM_InProgress == SM_Current_State) && (true == useSendPktStatus))
        {
            /* Make sure the message came from the SM module */
            if (pDataCnf->msduHandle & SM_FAIL_MSDU_HANDLE)
            {
                if(pDataCnf->status != ApiMac_status_success)
                {
                    /* Set an event that failure status from previous packet sent is seen */
                    Util_setEvent(&SM_events, SM_SEND_PKT_FB_EVT);
                }
                else
                {
                    /* Switch to fail state once fail packet is received */
                    Util_setEvent(&SM_events, SM_SENT_CM_FAIL_EVT);
                }
            }
            else if(pDataCnf->msduHandle & SM_DATA_MSDU_HANDLE)
            {
                if(pDataCnf->status != ApiMac_status_success)
                {
                    /* Set an event that failure status from previous packet sent is seen */
                    Util_setEvent(&SM_events, SM_SEND_PKT_FB_EVT);
                }
            }
        }
    }
#endif /* FEATURE_SECURE_COMMISSIONING */

    /* Make sure the message came from the app */
    if(pDataCnf->msduHandle & APP_MARKER_MSDU_HANDLE)
    {
        /* What message type was the original request? */
        if((pDataCnf->msduHandle & APP_MASK_MSDU_HANDLE)
           == APP_SENSOR_MSDU_HANDLE)
        {
#ifndef IEEE_COEX_TEST
            if(pDataCnf->status == ApiMac_status_success)
#endif
            {
                Sensor_msgStats.msgsSent++;
                if (Sensor_msgStats.msgsSent == 0)
                {
                    /* the count is wrapped around, set to 1 to avoid divided by zero */
                    Sensor_msgStats.msgsSent = 1;
                    totalE2EDelaySum = 0;
                }
#ifdef DISPLAY_PER_STATS
                Util_setEvent(&Sensor_events, SENSOR_UPDATE_STATS_EVT);
#endif /* DISPLAY_PER_STATS */
                /* Calculate end to end delay */
#ifdef OSAL_PORT2TIRTOS
                if(ClockP_getSystemTicks() < startSensorMsgTimeStamp)
                {
                    endToEndDelay = ClockP_getSystemTicks() +
                                    (0xFFFFFFFF-startSensorMsgTimeStamp);
                }
                else
                {
                    endToEndDelay = ClockP_getSystemTicks() - startSensorMsgTimeStamp;
                }
#else
                if(ICall_getTicks() < startSensorMsgTimeStamp)
                {
                    endToEndDelay = ICall_getTicks() +
                                    (0xFFFFFFFF-startSensorMsgTimeStamp);
                }
                else
                {
                    endToEndDelay = ICall_getTicks() - startSensorMsgTimeStamp;
                }
#endif
                endToEndDelay = (endToEndDelay + TICKPERIOD_MS_US - 1)/TICKPERIOD_MS_US;
                if ( (totalE2EDelaySum + endToEndDelay ) < totalE2EDelaySum)
                {
                    /* totalE2EDelaySum is wrapped around,reset the sent count 1 */
                    totalE2EDelaySum = endToEndDelay;
                    Sensor_msgStats.msgsSent = 1;
                }
                else
                {
                    totalE2EDelaySum += endToEndDelay;
                }

                Sensor_msgStats.worstCaseE2EDelay =
                   (Sensor_msgStats.worstCaseE2EDelay > endToEndDelay) ?
                    Sensor_msgStats.worstCaseE2EDelay:endToEndDelay;

                Sensor_msgStats.avgE2EDelay=totalE2EDelaySum/Sensor_msgStats.msgsSent;

                // if(g_rawTxState == RAW_TX_STATE_WAIT_ACK_COMPLETE_RESP) {
                //     Util_setEvent(&Sensor_events, SENSOR_SEND_RAW_DATA_EVT);
                // }
            }

#if CERTIFICATION_TEST_MODE
            {
                /* Setup for the next message */
                Ssf_setReadingClock(CERT_MODE_INTER_PKT_INTERVAL);
            }
#endif
        }
        if((pDataCnf->msduHandle & APP_MASK_MSDU_HANDLE)
           == APP_TRACKRSP_MSDU_HANDLE)
        {
            if(pDataCnf->status == ApiMac_status_success)
            {
                Sensor_msgStats.trackingResponseSent++;
            }
        }
        if((pDataCnf->msduHandle & APP_MASK_MSDU_HANDLE)
           == APP_CONFIGRSP_MSDU_HANDLE)
        {
            if(pDataCnf->status == ApiMac_status_success)
            {
                Sensor_msgStats.configResponseSent++;
            }
        }
    }
}

/*!
 * @brief      MAC Data Indication callback.
 *
 * @param      pDataInd - pointer to the data indication information
 */
static void dataIndCB(ApiMac_mcpsDataInd_t *pDataInd)
{
    uint8_t cmdBytes[SMSGS_TOGGLE_LED_RESPONSE_MSG_LEN];

    if((pDataInd != NULL) && (pDataInd->msdu.p != NULL)
       && (pDataInd->msdu.len > 0))
    {
        Smsgs_cmdIds_t cmdId = (Smsgs_cmdIds_t)*(pDataInd->msdu.p);

#ifdef FEATURE_MAC_SECURITY
        {
            if(Jdllc_securityCheck(&(pDataInd->sec)) == false)
            {
                /* reject the message */
                return;
            }
        }
#endif /* FEATURE_MAC_SECURITY */

        DEBUG_PRINT("Rec cmdId: %d, len: %d", cmdId, pDataInd->msdu.len);
        // dump pDataInd->msdu.p
        for(int i=0; i<pDataInd->msdu.len; i++){
            Display_printf(display, 0, 0, " %02x", pDataInd->msdu.p[i]);
            // if(i % 16 == 0){
            //    Display_printf(display, 0, 0, "\r\n"); 
            // }
        }
        // Display_printf(display, 0, 0, "\r\n");
        // Display_printf(display, 0, 0, "Data: %02x %02x %02x %02x %02x %02x %02x %02x \n%02x %02x %02x %02x %02x %02x %02x %02x \n", 
        //         pDataInd->msdu.p[0], pDataInd->msdu.p[1], pDataInd->msdu.p[2], pDataInd->msdu.p[3], pDataInd->msdu.p[4], pDataInd->msdu.p[5], pDataInd->msdu.p[6], pDataInd->msdu.p[7], 
        //         pDataInd->msdu.p[8], pDataInd->msdu.p[9], pDataInd->msdu.p[10], pDataInd->msdu.p[11], pDataInd->msdu.p[12], pDataInd->msdu.p[13], pDataInd->msdu.p[14], pDataInd->msdu.p[15]);

        switch(cmdId)
        {
            case Smsgs_cmdIds_configReq:
                processConfigRequest(pDataInd);
                Sensor_msgStats.configRequests++;
                break;

            case Smsgs_cmdIds_trackingReq:
                /* Make sure the message is the correct size */
#ifndef IEEE_COEX_TEST
                if(pDataInd->msdu.len == SMSGS_TRACKING_REQUEST_MSG_LENGTH)
#endif
                {
                    /* only send data if sensor is in the network */
                    if ((Jdllc_getProvState() == Jdllc_states_joined) ||
                            (Jdllc_getProvState() == Jdllc_states_rejoined))
                    {
                        /* Update stats */
                        Sensor_msgStats.trackingRequests++;
#ifdef DISPLAY_PER_STATS
                        Ssf_displayRxStats(pDataInd->rssi);
#endif // DISPLAY_PER_STATS
                        /* Indicate tracking message received */
                        Ssf_trackingUpdate(&pDataInd->srcAddr);
#if (defined(IEEE_COEX_TEST) && defined(EN_TRACKINGRSP)) || !defined(IEEE_COEX_TEST)
                        /* send the response message directly */
                        cmdBytes[0] = (uint8_t) Smsgs_cmdIds_trackingRsp;

                        Sensor_sendMsg(Smsgs_cmdIds_trackingRsp,
                                           &pDataInd->srcAddr, true,
                                           1, cmdBytes);
#endif
                    }
                }
                break;

            case Smsgs_cmdIds_toggleLedReq:
                /* Make sure the message is the correct size */
                if(pDataInd->msdu.len == SMSGS_TOGGLE_LED_REQUEST_MSG_LEN)
                {
                    /* only send data if sensor is in the network */
                    if ((Jdllc_getProvState() == Jdllc_states_joined) ||
                            (Jdllc_getProvState() == Jdllc_states_rejoined))
                    {
                        /* send the response message directly */
                        cmdBytes[0] = (uint8_t) Smsgs_cmdIds_toggleLedRsp;
                        cmdBytes[1] = Ssf_toggleLED();
                        Sensor_sendMsg(Smsgs_cmdIds_toggleLedRsp,
                            &pDataInd->srcAddr, true,
                            SMSGS_TOGGLE_LED_RESPONSE_MSG_LEN,
                            cmdBytes);
                    }
                }
                break;

            case Smgs_cmdIds_broadcastCtrlMsg:
                if(parentFound)
                {
                    /* Node has successfully associated with the network */
                    processBroadcastCtrlMsg(pDataInd);
                }
                break;

            case Smsgs_cmdIds_rampdata:
                // Sensor_pwrMeasStats.rampDataRcvd++;
                PacketTransmission_processRampResp(pDataInd);
                break;

            case Smsgs_cmdIds_sensorData:
                /* Handle sensor data message with new configuration fields */
                processSensorDataMessage(pDataInd);
                break;

            case Smsgs_cmdIds_RebootCmd:
                /* Reboot command: cmdId(1B) + delay(2B, ms) */
                if(pDataInd->msdu.len == SMSGS_REBOOT_CMD_MSG_LEN)
                {
                    uint8_t *pBuf = pDataInd->msdu.p;
                    Smsgs_rebootCmd_t rebootCmd;

                    rebootCmd.cmdId = (Smsgs_cmdIds_t)*pBuf++;
                    rebootCmd.delay = Util_parseUint16(pBuf);

                    /* Immediately send reboot response */
                    DataSender_sendRebootRsp(&pDataInd->srcAddr, 0);

                    /* Schedule delayed reboot */
                    Sensor_scheduleReboot(rebootCmd.delay);
                }
                break;

#ifdef FEATURE_NATIVE_OAD
            case Smsgs_cmdIds_oad:
                //Index past the Smsgs_cmdId
                OADProtocol_ParseIncoming((void*) &(pDataInd->srcAddr), pDataInd->msdu.p + 1);
                break;
#endif //FEATURE_NATIVE_OAD
#ifdef FEATURE_SECURE_COMMISSIONING
            case Smgs_cmdIds_CommissionStart:
                {
                    ApiMac_sec_t devSec;
                    extern ApiMac_sAddrExt_t ApiMac_extAddr;

                    /* Obtain MAC level security information. Use network key for SM */
                    Jdllc_securityFill(&devSec);

                    uint8_t *pBuf = pDataInd->msdu.p;
                    pBuf += sizeof(Smsgs_cmdIds_t);
                    SMMsgs_cmdIds_t CMMsgId = (SMMsgs_cmdIds_t)Util_buildUint16(pBuf[0], pBuf[1]);

                    /* read the current value */
                    ApiMac_mlmeGetReqBool(ApiMac_attribute_autoRequest, &currAutoReq);

                    /* beacon-mode of operation and autoRequest is set to true and sleepy device  */
                    if((CONFIG_MAC_BEACON_ORDER != 15) && (currAutoReq == true) && (!CONFIG_RX_ON_IDLE))
                    {
                        /* set autoRequest to false and enable explicit polling */
                        ApiMac_mlmeSetReqBool(ApiMac_attribute_autoRequest, false);
                        Util_setEvent(&Jdllc_events, JDLLC_POLL_EVT);
                    }

                    if (CMMsgId == SMMsgs_cmdIds_keyRefreshRequest)
                    {
                        if(SM_Current_State != SM_CM_InProgress)
                        {
                            /* Kick off key refreshment process if SM state machine is not being run */
                            SM_startKeyRefreshProcess(&parentInfo.devInfo, &devSec,
                                                               parentInfo.fh, true);
                        }
                    }
                    else if(CMMsgId == SMMsgs_cmdIds_processRequest)
                    {
                        SM_startCMProcess(&parentInfo.devInfo, &devSec, parentInfo.fh,
                                          true, SM_type_device, smAuthMethod);
                    }
                }
                break;
            case Smgs_cmdIds_CommissionMsg:
                {
                    /* Process Security manager commissioning data */
                    SM_processCommData(pDataInd);
                }
                break;


#endif /* FEATURE_SECURE_COMMISSIONING */

#if defined(DEVICE_TYPE_MSG)
            case Smsgs_cmdIds_DeviceTypeReq:
                /* Make sure the message is the correct size */
                if(pDataInd->msdu.len == SMSGS_DEVICE_TYPE_REQUEST_MSG_LEN)
                {
                    /* Only send data if sensor is in the network */
                    if ((Jdllc_getProvState() == Jdllc_states_joined) ||
                            (Jdllc_getProvState() == Jdllc_states_rejoined))
                    {
                        Sensor_sendDeviceTypeResponse();
                    }
                }
                break;
#endif /* DEVICE_TYPE_MSG */

            default:
                /* Should not receive other messages */
                break;
        }
    }
}

/*!
 * @brief      Get the next MSDU Handle
 *             <BR>
 *             The MSDU handle has 3 parts:<BR>
 *             - The MSBit(7), when set means the the application sent the
 *               message
 *             - Bit 6, when set means that the app message is a config request
 *             - Bits 0-5, used as a message counter that rolls over.
 *
 * @param      msgType - message command id needed
 *
 * @return     msdu Handle
 */
static uint8_t getMsduHandle(Smsgs_cmdIds_t msgType)
{
    uint8_t msduHandle = deviceTxMsduHandle;

    /* Increment for the next msdu handle, or roll over */
    if(deviceTxMsduHandle >= MSDU_HANDLE_MAX)
    {
        deviceTxMsduHandle = 0;
    }
    else
    {
        deviceTxMsduHandle++;
    }

    /* Add the App specific bit */
    msduHandle |= APP_MARKER_MSDU_HANDLE;

    /* Add the message type bit */
    if(msgType == Smsgs_cmdIds_sensorData || msgType == Smsgs_cmdIds_rampdata)
    {
        msduHandle |= APP_SENSOR_MSDU_HANDLE;
    }
    else if(msgType == Smsgs_cmdIds_trackingRsp)
    {
        msduHandle |= APP_TRACKRSP_MSDU_HANDLE;
    }
    else if(msgType == Smsgs_cmdIds_configRsp)
    {
        msduHandle |= APP_CONFIGRSP_MSDU_HANDLE;
    }

    return (msduHandle);
}

#if !defined(OAD_IMG_A) && !defined(POWER_MEAS)

/*!
 @brief   Build and send sensor data message
 */
static bool processSensorMsgEvt(uint16_t frameControl)
{
    Smsgs_sensorMsg_t sensor;

    memset(&sensor, 0, sizeof(Smsgs_sensorMsg_t));

    ApiMac_mlmeGetReqArray(ApiMac_attribute_extendedAddress,
                           sensor.extAddress);

    /* fill in the message */
    sensor.frameControl = frameControl;

    if(sensor.frameControl & Smsgs_dataFields_msgStats)
    {
        memcpy(&sensor.msgStats, &Sensor_msgStats,
               sizeof(Smsgs_msgStatsField_t));
    }
    if(sensor.frameControl & Smsgs_dataFields_configSettings)
    {
        sensor.configSettings.pollingInterval = configSettings.pollingInterval;
        sensor.configSettings.reportingInterval = configSettings
                        .reportingInterval;
    }

    /* 以下是自定义数据，注意发送长度不能超过100 */

    if(sensor.frameControl & Smsgs_dataFields_accelSingleAxisSensor)
    {
        memcpy(&sensor.vibrateSensor, &vibrateSensor,
               sizeof(Smsgs_vibrateSensorField_t));
    }
    
    if(sensor.frameControl & Smsgs_dataFields_accelThreeAxisSensor)
    {
        memcpy(&sensor.vibrateSensor, &vibrateSensor,
               sizeof(Smsgs_vibrateSensorField_t));
    }

    /*if(sensor.frameControl & Smsgs_dataFields_tempAndBattery) {
        memcpy(&sensor.vibrateTemplateBat, &vibrateTemplateBat,
               sizeof(Smsgs_vibrateTemplateBat_t));
    }*/
    
    if(sensor.frameControl & Smsgs_dataFields_samplingConfig) {
        memcpy(&sensor.vibrateSampleConfig, &vibrateSampleConfig,
               sizeof(Smsgs_vibrateSampleConfig_t));
        // configSettings.frameControl &= ~Smsgs_dataFields_samplingConfig;  //清除采样配置帧控制位，只发送一次
    }
    
    if(sensor.frameControl & Smsgs_dataFields_channelConfig) {
        memcpy(&sensor.vibrateChannelConfig, &vibrateChannelConfig,
               sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM);
        // configSettings.frameControl &= ~Smsgs_dataFields_channelConfig;  //清除通道配置帧控制位，只发送一次
    }
    
    if(sensor.frameControl & Smsgs_dataFields_channelTempConfig) {
        memcpy(&sensor.vibrateChannelTempConfig, &vibrateChannelTempConfig,
               sizeof(Smsgs_vibrateChannelTempConfig_t) * SENSOR_TEMPALTE_NUM);
        // configSettings.frameControl &= ~Smsgs_dataFields_channelTempConfig;  //清除通道温度配置帧控制位，只发送一次
    }

    /* inform the user interface */
    Ssf_sensorReadingUpdate(&sensor);

    /* send the data to the collector */
    return DataSender_sendSensorMessage(&collectorAddr, &sensor);
}

/*!
 * @brief   Manually read the sensors
 */
static void readSensors(void)
{
#if defined(VIBRATE_SENSOR)
    // TODO 读取震动传感器数据
    // configSettings.frameControl |= Smsgs_dataFields_tempAndBattery;
    // configSettings.frameControl |= Smsgs_dataFields_samplingConfig;
    // configSettings.frameControl |= Smsgs_dataFields_channelConfig;
    // configSettings.frameControl |= Smsgs_dataFields_channelTempConfig;
#endif
}

#endif /* !defined(OAD_IMG_A) && !defined(POWER_MEAS) */

/*!
 * @brief      Process the Config Request message.
 *
 * @param      pDataInd - pointer to the data indication information
 */
static void processConfigRequest(ApiMac_mcpsDataInd_t *pDataInd)
{
    Smsgs_statusValues_t stat = Smsgs_statusValues_invalid;
    Smsgs_configRspMsg_t configRsp;

    memset(&configRsp, 0, sizeof(Smsgs_configRspMsg_t));

    /* Make sure the message is the correct size */
    if(pDataInd->msdu.len == SMSGS_CONFIG_REQUEST_MSG_LENGTH)
    {
        uint8_t *pBuf = pDataInd->msdu.p;
        uint16_t frameControl;
        uint32_t reportingInterval;
        uint32_t pollingInterval;

        /* Parse the message */
        // 小端模式
        configSettings.cmdId = (Smsgs_cmdIds_t)*pBuf++;
        frameControl = *((uint16_t *)pBuf);
        pBuf += 2;
        reportingInterval = *((uint32_t *)pBuf);
        pBuf += 4;
        pollingInterval = *((uint32_t *)pBuf);

        stat = Smsgs_statusValues_success;
        collectorAddr.addrMode = pDataInd->srcAddr.addrMode;
        if(collectorAddr.addrMode == ApiMac_addrType_short)
        {
            collectorAddr.addr.shortAddr = pDataInd->srcAddr.addr.shortAddr;
        }
        else
        {
            memcpy(collectorAddr.addr.extAddr, pDataInd->srcAddr.addr.extAddr,
                   (APIMAC_SADDR_EXT_LEN));
        }

        configSettings.frameControl = validateFrameControl(frameControl);
        if(configSettings.frameControl != frameControl)
        {
            stat = Smsgs_statusValues_partialSuccess;
        }
        configRsp.frameControl = configSettings.frameControl;

        if((reportingInterval < MIN_REPORTING_INTERVAL)
           || (reportingInterval > MAX_REPORTING_INTERVAL))
        {
            stat = Smsgs_statusValues_partialSuccess;
        }
        else
        {
#ifndef POWER_MEAS
            configSettings.reportingInterval = reportingInterval;
#endif
            {
                uint32_t randomNum;
                randomNum = ((ApiMac_randomByte() << 16) +
                             (ApiMac_randomByte() << 8) + ApiMac_randomByte());
                randomNum = (randomNum % reportingInterval) +
                        SENSOR_MIN_POLL_TIME;
#if !defined(IEEE_COEX_TEST) || !defined(COEX_MENU)
                Ssf_setReadingClock(randomNum);
#endif
            }

        }
        configRsp.reportingInterval = configSettings.reportingInterval;

        if((pollingInterval < MIN_POLLING_INTERVAL)
           || (pollingInterval > MAX_POLLING_INTERVAL))
        {
            stat = Smsgs_statusValues_partialSuccess;
        }
        else
        {
            configSettings.pollingInterval = pollingInterval;
            Jdllc_setPollRate(configSettings.pollingInterval);
        }
        configRsp.pollingInterval = configSettings.pollingInterval;
    }

    /* Send the response message */
    configRsp.cmdId = Smsgs_cmdIds_configRsp;
    configRsp.status = stat;

    /* Update the user */
    Ssf_configurationUpdate(&configRsp);

    /* Response the the source device */
    DataSender_sendConfigRsp(&pDataInd->srcAddr, &configRsp);
}

/*!
 * @brief      Process the Broadcast Control Msg.
 *
 * @param      pDataInd - pointer to the data indication information
 */
static void processBroadcastCtrlMsg(ApiMac_mcpsDataInd_t *pDataInd)
{
    Smsgs_broadcastcmdmsg_t broadcastCmd;

    memset(&broadcastCmd, 0, sizeof(Smsgs_broadcastcmdmsg_t));

    /* Make sure the message is the correct size */
    if(pDataInd->msdu.len == SMSGS_BROADCAST_CMD_LENGTH)
    {
        uint8_t *pBuf = pDataInd->msdu.p;
        uint16_t broadcastMsgId;

        /* Parse the message */
        uint8_t cmdId = (Smsgs_cmdIds_t)*pBuf++;
        broadcastMsgId = Util_parseUint16(pBuf);

        /* Process Broadcast Command Message */
        Sensor_msgStats.numBroadcastMsgRcvd++;

        if(!initBroadcastMsg)
        {
            /* Not the first broadcast msg rcvdd after join or a rejoin*/
            if((broadcastMsgId - lastRcvdBroadcastMsgId) > 1)
            {
                Sensor_msgStats.numBroadcastMsglost +=
                                ((broadcastMsgId - lastRcvdBroadcastMsgId) -1);
            }
        }

        lastRcvdBroadcastMsgId = broadcastMsgId;
        /*To handle the very first broadcast msg rcvdd after join or a rejoin*/
        initBroadcastMsg = false;

        /* Switch On or Off LED based on broadcast Msg Id */
        if((broadcastMsgId % 2) == 0)
        {
            Ssf_OnLED();
        }
        else
        {
            Ssf_OffLED();
        }
    }
}


/*!
 * @brief   Build and send Config Response message
 *
 * @param   pDstAddr - Where to send the message
 * @param   pMsg - pointer to the Config Response
 *
 * @return  true if message was sent, false if not
 */
static bool sendConfigRsp(ApiMac_sAddr_t *pDstAddr, Smsgs_configRspMsg_t *pMsg)
{
    uint8_t msgBuf[SMSGS_CONFIG_RESPONSE_MSG_LENGTH];
    uint8_t *pBuf = msgBuf;

    *pBuf++ = (uint8_t) Smsgs_cmdIds_configRsp;
    pBuf = Util_bufferUint16(pBuf, pMsg->status);
    pBuf = Util_bufferUint16(pBuf, pMsg->frameControl);
    pBuf = Util_bufferUint32(pBuf, pMsg->reportingInterval);
    pBuf = Util_bufferUint32(pBuf, pMsg->pollingInterval);

    return (Sensor_sendMsg(Smsgs_cmdIds_configRsp, pDstAddr, true,
                    SMSGS_CONFIG_RESPONSE_MSG_LENGTH, msgBuf));
}

/*!
 * @brief   Filter the frameControl with readings supported by this device.
 *
 * @param   frameControl - suggested frameControl
 *
 * @return  new frame control settings supported
 */
static uint16_t validateFrameControl(uint16_t frameControl)
{
    uint16_t newFrameControl = 0;

    if(frameControl & Smsgs_dataFields_msgStats)
    {
        newFrameControl |= Smsgs_dataFields_msgStats;
    }
    if(frameControl & Smsgs_dataFields_configSettings)
    {
        newFrameControl |= Smsgs_dataFields_configSettings;
    }
#if defined(VIBRATE_SENSOR)
    if(frameControl & Smsgs_dataFields_accelSingleAxisSensor)
    {
        newFrameControl |= Smsgs_dataFields_accelSingleAxisSensor;
    }
    if(frameControl & Smsgs_dataFields_accelThreeAxisSensor)
    {
        newFrameControl |= Smsgs_dataFields_accelThreeAxisSensor;
    }
    if(frameControl & Smsgs_dataFields_tempAndBattery)
    {
        newFrameControl |= Smsgs_dataFields_tempAndBattery;
    }
    if(frameControl & Smsgs_dataFields_samplingConfig){
        newFrameControl |= Smsgs_dataFields_samplingConfig;
    }
    if(frameControl & Smsgs_dataFields_channelConfig)
    {
        newFrameControl |= Smsgs_dataFields_channelConfig;
    }
    if(frameControl & Smsgs_dataFields_channelTempConfig)
    {
        newFrameControl |= Smsgs_dataFields_channelTempConfig;
    }
#endif

#ifdef DMM_CENTRAL
    if(frameControl & Smsgs_dataFields_bleSensor)
    {
        newFrameControl |= Smsgs_dataFields_bleSensor;
    }
#endif

    return (newFrameControl);
}

#if defined(DEVICE_TYPE_MSG)
static void Sensor_sendDeviceTypeResponse(void)
{
    DataSender_sendDeviceTypeResponse(&collectorAddr);
}
#endif /* DEVICE_TYPE_MSG */

/*!
 * @brief   The device joined callback.
 *
 * @param   pDevInfo - This device's information
 * @param   pParentInfo - This is the parent's information
 */
static void jdllcJoinedCb(ApiMac_deviceDescriptor_t *pDevInfo,
                          Llc_netInfo_t *pParentInfo)
{
    uint32_t randomNum = 0;

    /* Copy the parent information */
    memcpy(&parentInfo, pParentInfo, sizeof(Llc_netInfo_t));

    /* Set the collector's address as the parent's address */
    if (pParentInfo->fh && CONFIG_RX_ON_IDLE)
    {
        collectorAddr.addrMode = ApiMac_addrType_extended;
        memcpy(collectorAddr.addr.extAddr, pParentInfo->devInfo.extAddress,
               (APIMAC_SADDR_EXT_LEN));
    }
    else
    {
        collectorAddr.addrMode = ApiMac_addrType_short;
        collectorAddr.addr.shortAddr = pParentInfo->devInfo.shortAddress;
    }

    /* Start the reporting timer */
    if(CONFIG_FH_ENABLE)
    {
        randomNum = ((ApiMac_randomByte() << 16) +
                     (ApiMac_randomByte() << 8) + ApiMac_randomByte());
        randomNum = (randomNum % configSettings.reportingInterval) +
                    SENSOR_MIN_POLL_TIME;
        Ssf_setReadingClock(randomNum);
    }
    else
    {
       uint32_t randomNum;
       randomNum = ((ApiMac_randomByte() << 16) +
                    (ApiMac_randomByte() << 8) + ApiMac_randomByte());
       randomNum = (randomNum % configSettings.reportingInterval ) +
                   SENSOR_MIN_POLL_TIME;
#if !defined(IEEE_COEX_TEST) || !defined(COEX_MENU)
       Ssf_setReadingClock(randomNum);
#endif
        // ConfigSendStateMachine_start();
    }

    /* Inform the user of the joined information */
    Ssf_networkUpdate(rejoining, pDevInfo, pParentInfo);

    // 载入默认发送数据类型到队列
    Sensor_loadDefaultFrameControl();

#ifdef FEATURE_SECURE_COMMISSIONING
        SM_Sensor_SAddress = pDevInfo->shortAddress;
#endif /* FEATURE_SECURE_COMMISSIONING */
    if((rejoining == false) && (pParentInfo->fh == false))
    {
#ifdef FEATURE_MAC_SECURITY
        ApiMac_status_t stat;
        /* Add the parent to the security device list */
        stat = Jdllc_addSecDevice(pParentInfo->devInfo.panID,
                                  pParentInfo->devInfo.shortAddress,
                                  &pParentInfo->devInfo.extAddress, 0);
        if(stat != ApiMac_status_success)
        {
            Ssf_displayError("Auth Error: 0x", (uint8_t)stat);
        }
#endif /* FEATURE_MAC_SECURITY */
    }

#if (CONFIG_MAC_SUPERFRAME_ORDER != 15) && defined(MAC_NO_AUTO_REQ)
    /*
     * Set MAC Auto Request to false to enable multiple poll requests
     * per beacon interval
     */
    ApiMac_mlmeSetReqBool(ApiMac_attribute_autoRequest, false);
#endif

#ifdef OSAL_PORT2TIRTOS
    /* Calculate Join Time */
    if(ClockP_getSystemTicks() < joinTimeTicks)
    {
        joinTimeTicks = ClockP_getSystemTicks() + (0xFFFFFFFF-joinTimeTicks);
    }
    else
    {
        joinTimeTicks = ClockP_getSystemTicks() - joinTimeTicks;
    }
#else
    /* Calculate Join Time */
    if(ICall_getTicks() < joinTimeTicks)
    {
        joinTimeTicks = ICall_getTicks() + (0xFFFFFFFF-joinTimeTicks);
    }
    else
    {
        joinTimeTicks = ICall_getTicks() - joinTimeTicks;
    }
#endif
    Sensor_msgStats.joinTime = joinTimeTicks / TICKPERIOD_MS_US;
#ifdef DISPLAY_PER_STATS
    /* clear the stats used for PER so that we start out at a
     * zeroed state
     */
    Sensor_msgStats.macAckFailures = 0;
    Sensor_msgStats.otherDataRequestFailures = 0;
    Sensor_msgStats.msgsSent = 0;
#endif
}

/*!
 * @brief   Disassociation indication callback.
 *
 * @param   pExtAddress - extended address
 * @param   reason - reason for disassociation
 */
static void jdllcDisassocIndCb(ApiMac_sAddrExt_t *pExtAddress,
                               ApiMac_disassocateReason_t reason)
{
    /* Stop the reporting timer */
    Ssf_setReadingClock(0);
    Ssf_clearNetworkInfo();

#ifdef FEATURE_MAC_SECURITY
    ApiMac_secDeleteDevice(pExtAddress);
#endif

#ifdef FEATURE_SECURE_COMMISSIONING
    SM_removeEntryFromSeedKeyTable(&ApiMac_extAddr);
    Ssf_clearDeviceKeyInfo();
#endif /* FEATURE_SECURE_COMMISSIONING */

#ifdef FEATURE_NATIVE_OAD
    /* OAD abort with no auto resume */
    OADClient_abort(false);
#endif //FEATURE_NATIVE_OAD
}

/*!
 * @brief   Disassociation confirm callback to an application intiated
 *          disassociation request.
 *
 * @param   pExtAddress - extended address
 * @param   status - status of disassociation
 */
static void jdllcDisassocCnfCb(ApiMac_sAddrExt_t *pExtAddress,
                               ApiMac_status_t status)
{
    /* Stop the reporting timer */
    Ssf_setReadingClock(0);
    Ssf_clearNetworkInfo();

#ifdef FEATURE_MAC_SECURITY
    ApiMac_secDeleteDevice(pExtAddress);
#endif

#ifdef FEATURE_SECURE_COMMISSIONING
    SM_removeEntryFromSeedKeyTable(&ApiMac_extAddr);
    Ssf_clearDeviceKeyInfo();
#endif /* FEATURE_SECURE_COMMISSIONING */
#ifdef FEATURE_NATIVE_OAD
    /* OAD abort with no auto resume */
    OADClient_abort(false);
#endif //FEATURE_NATIVE_OAD
}

/*!
 * @brief   JDLLC state change callback.
 *
 * @param   state - new state
 */
static void jdllcStateChangeCb(Jdllc_states_t state)
{
#ifdef FEATURE_NATIVE_OAD
    if( (state == Jdllc_states_joined) || (state == Jdllc_states_rejoined))
    {
#if (CONFIG_MAC_SUPERFRAME_ORDER == 15)
        /* resume an OAD that may have aborted */
        OADClient_resume(30000);
#else
        /* resume an OAD that may have aborted */
        OADClient_resume(60000);
#endif
    }
    else if(state == Jdllc_states_orphan)
    {
        /* OAD abort with no auto resume */
        OADClient_abort(false);
    }
#endif /* FEATURE_NATIVE_OAD */

// #ifndef CUI_DISABLE
    Ssf_stateChangeUpdate(state);
// #endif /* CUI_DISABLE */

#ifdef OAD_IMG_A
    if( (state == Jdllc_states_joined) || (state == Jdllc_states_rejoined))
    {
        Util_setEvent(&Sensor_events, SENSOR_OAD_SEND_RESET_RSP_EVT);
    }
#endif /* OAD_IMG_A */
}

#ifdef FEATURE_SECURE_COMMISSIONING
/*!
 * @brief      Security manager failure processing function
 */
void smFailCMProcessCb(ApiMac_deviceDescriptor_t *devInfo, bool rxOnIdle,
                       bool keyRefreshment, SMMsgs_errorCode_t errorCode)
{
    /* restore, write back current Pib value for auto request attribute */
    ApiMac_mlmeSetReqBool(ApiMac_attribute_autoRequest, currAutoReq);

    if (SM_forceStopped == true)
    {
        Util_setEvent(&Sensor_events, SENSOR_DISASSOC_EVT);
    }
}

/*!
 * @brief      Security manager success processing function
 */
void smSuccessCMProcessCb(ApiMac_deviceDescriptor_t *devInfo, bool keyRefreshment)
{
    /* restore, write back current Pib value for auto request attribute */
    ApiMac_mlmeSetReqBool(ApiMac_attribute_autoRequest, currAutoReq);
}

#endif /* FEATURE_SECURE_COMMISSIONING */

/*!
 * @brief   Process sensor data message with new configuration fields
 *
 * @param   pDataInd - pointer to the data indication information
 */
static void processSensorDataMessage(ApiMac_mcpsDataInd_t *pDataInd)
{
    if((pDataInd != NULL) && (pDataInd->msdu.p != NULL)
       && (pDataInd->msdu.len > 0))
    {
        /* Parse the sensor message */
        Smsgs_sensorMsg_t sensorMsg;
        uint8_t *pBuf = pDataInd->msdu.p;
        uint16_t frameControl;
        
        /* Parse basic message structure */
        sensorMsg.cmdId = (Smsgs_cmdIds_t)*pBuf++;
        // memcpy(sensorMsg.extAddress, pBuf, SMGS_SENSOR_EXTADDR_LEN);
        // pBuf += SMGS_SENSOR_EXTADDR_LEN;
        frameControl = Util_parseUint16(pBuf);
        pBuf += 2;
        sensorMsg.frameControl = frameControl;
        
        /* Check for sampling configuration data */
        if(frameControl & Smsgs_dataFields_samplingConfig)
        {
            if(pDataInd->msdu.len >= (3 + sizeof(Smsgs_vibrateSampleConfig_t)))
            {
                memcpy(&sensorMsg.vibrateSampleConfig, pBuf, sizeof(Smsgs_vibrateSampleConfig_t));
                pBuf += sizeof(Smsgs_vibrateSampleConfig_t);
                
                memcpy(&vibrateSampleConfig, &sensorMsg.vibrateSampleConfig, 
                    sizeof(Smsgs_vibrateSampleConfig_t));
                /* Save sampling configuration to NV */
                Ssf_samplingConfigUpdate(&sensorMsg);
                DEBUG_PRINT("Sampling config saved");
                // send response
                frameControl = Smsgs_dataFields_samplingConfig;
                Sensor_enqueueFrameControl(frameControl);
            }
        }
        
        /* Check for channel configuration data */
        if(frameControl & Smsgs_dataFields_channelConfig)
        {
            if(pDataInd->msdu.len >= (3 + sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM))
            {
                memcpy(&sensorMsg.vibrateChannelConfig, pBuf, sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM);
                pBuf += sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM;
                
                memcpy(&vibrateChannelConfig, &sensorMsg.vibrateChannelConfig, 
                    sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM);
                /* Save channel configuration to NV */
                Ssf_channelConfigUpdate(&sensorMsg);
                DEBUG_PRINT("Channel config saved");
                // send response
                frameControl = Smsgs_dataFields_channelConfig;
                Sensor_enqueueFrameControl(frameControl);
            }
        }
        
        /* Check for channel temperature compensation configuration data */
        if(frameControl & Smsgs_dataFields_channelTempConfig)
        {
            if(pDataInd->msdu.len >= (3 + sizeof(Smsgs_vibrateChannelTempConfig_t)))
            {
                memcpy(&sensorMsg.vibrateChannelTempConfig, pBuf, sizeof(Smsgs_vibrateChannelTempConfig_t));
                pBuf += sizeof(Smsgs_vibrateChannelTempConfig_t);
                
                memcpy(&vibrateChannelTempConfig, &sensorMsg.vibrateChannelTempConfig, 
                    sizeof(Smsgs_vibrateChannelTempConfig_t));
                /* Save channel temperature compensation configuration to NV */
                Ssf_channelTempConfigUpdate(&sensorMsg);
                DEBUG_PRINT("Channel temp config saved");
                // send response
                frameControl = Smsgs_dataFields_channelTempConfig;
                Sensor_enqueueFrameControl(frameControl);
            }
        }
        
        /* Update statistics */
        Sensor_msgStats.configRequests++;
    }
}

static void readSavedConfigurationData(void)
{
    Ssf_samplingConfig_t samplingConfig;
    Ssf_channelConfig_t channelConfig;
    Ssf_channelTempConfig_t channelTempConfig;
    
    /* 读取保存的采样配置数据 */
    if(Ssf_getSamplingConfigInfo(&samplingConfig)) {
        DEBUG_PRINT("Loading saved sampling config");
        memcpy(&vibrateSampleConfig, &samplingConfig.vibrateSampleConfig, 
               sizeof(Smsgs_vibrateSampleConfig_t));
    } else {
        DEBUG_PRINT("No saved sampling config found");
    }
    
    /* 读取保存的通道配置数据 */
    if(Ssf_getChannelConfigInfo(&channelConfig)) {
        DEBUG_PRINT("Loading saved channel config");
        memcpy(&vibrateChannelConfig, &channelConfig.vibrateChannelConfig, 
               sizeof(Smsgs_vibrateChannelConfig_t) * SENSOR_CHANNEL_NUM);
    } else {
        DEBUG_PRINT("No saved channel config found");
    }
    
    /* 读取保存的通道温度补偿配置数据 */
    if(Ssf_getChannelTempConfigInfo(&channelTempConfig)) {
        DEBUG_PRINT("Loading saved channel temp config");
        memcpy(&vibrateChannelTempConfig, &channelTempConfig.vibrateChannelTempConfig, 
               sizeof(Smsgs_vibrateChannelTempConfig_t) * SENSOR_TEMPALTE_NUM);
    } else {
        DEBUG_PRINT("No saved channel temp config found");
    }
}


/*!
 * @brief 进入低功耗通知函数
 * 
 * @param eventType - 事件类型
 * @param eventArg - 事件参数
 * @param clientArg - 客户端参数
 * 
 * @return Power_NOTIFYDONE - 通知成功
 */
static int_fast16_t Sensor_postNotifyFxn_entering_standby(unsigned int eventType, uintptr_t eventArg, uintptr_t clientArg)
{
    // DEBUG_PRINT("Sensor_postNotifyFxn_entering_standby: eventType = %d", eventType);
    // Ssf_OffDataLED();
    return Power_NOTIFYDONE;
}

/*!
 * @brief 唤醒低功耗通知函数
 * 
 * @param eventType - 事件类型
 * @param eventArg - 事件参数
 * @param clientArg - 客户端参数
 * 
 * @return Power_NOTIFYDONE - 通知成功
 */
static int_fast16_t Sensor_postNotifyFxn_awake_standby(unsigned int eventType, uintptr_t eventArg, uintptr_t clientArg)
{
    // DEBUG_PRINT("Sensor_postNotifyFxn_awake_standby: eventType = %d", eventType);
    // Ssf_OnDataLED();
    return Power_NOTIFYDONE;
}